#include <rclcpp/rclcpp.hpp>
#include "bumperbot_msgs/srv/add_two_ints.hpp"

using std::placeholders::_1;
using std::placeholders::_2;

class SimpleServiceServer : public rclcpp::Node
{
public:
  SimpleServiceServer() : Node("simple_service_server")
  {
    service_ = create_service<bumperbot_msgs::srv::AddTwoInts>("add_two_ints",
      std::bind(&SimpleServiceServer::serviceCallback, this, _1, _2)
    );

    RCLCPP_INFO(get_logger(), "Service add_two_ints Ready");
  }

private:
  void serviceCallback(
    std::shared_ptr<bumperbot_msgs::srv::AddTwoInts::Request> req,
    std::shared_ptr<bumperbot_msgs::srv::AddTwoInts::Response> res
  )
  {
    RCLCPP_INFO_STREAM(get_logger(), 
      "Request Received a: " << req->a << ", b: " << req->b);
    
    res->sum = req->a + req->b;
    RCLCPP_INFO_STREAM(get_logger(), "Returning sum: " << res->sum);
  }

  rclcpp::Service<bumperbot_msgs::srv::AddTwoInts>::SharedPtr service_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto service_server_node = std::make_shared<SimpleServiceServer>();
  rclcpp::spin(service_server_node);
  rclcpp::shutdown();

  return 0;
}